#include <zephyr/kernel.h>
#include <zephyr/device.h>
#include <zephyr/devicetree.h>
#include <zephyr/drivers/gpio.h>
#include "include/pca9685.h"
#include <zephyr/drivers/i2c.h>
#include <math.h>
#include <zephyr/logging/log.h>
LOG_MODULE_REGISTER(pca9685);


extern const struct device *const i2c_dev;
extern const struct gpio_dt_spec *iicaddr_io_1, *iicaddr_io_2, *iicaddr_io_3, *iicaddr_io_4, *iicaddr_io_5, *iicaddr_io_6;

double my_floor(double x)
{
   double y=x;
    if( (*( ( (int *) &y)+1) & 0x80000000)  != 0) //或者if(x<0)
        return (float)((int)x)-1;

    else
        return (float)((int)x);
}


static int write_bytes(const struct device *i2c_dev, uint8_t addr,
		       uint8_t *data, uint32_t num_bytes)
{
	struct i2c_msg msgs[2];

	/* Send the address to write to */
	msgs[0].buf = &addr;
	msgs[0].len = 1U;
	msgs[0].flags = I2C_MSG_WRITE;

	/* Data to be written, and STOP after this. */
	msgs[1].buf = data;
	msgs[1].len = num_bytes;
	msgs[1].flags = I2C_MSG_WRITE | I2C_MSG_STOP;

	return i2c_transfer(i2c_dev, &msgs[0], 2, PCA9685_adrr);
}

static int read_bytes(const struct device *i2c_dev, uint8_t addr,
		      uint8_t *data, uint32_t num_bytes)
{
	struct i2c_msg msgs[2];

	/* Setup I2C messages */

	/* Send the address to read from */
	msgs[0].buf = &addr;
	msgs[0].len = 1U;
	msgs[0].flags = I2C_MSG_WRITE | I2C_MSG_STOP;
    i2c_transfer(i2c_dev, &msgs[0], 1, PCA9685_adrr);
    k_busy_wait(10);
	/* Read from device. STOP after this. */
	msgs[1].buf = data;
	msgs[1].len = num_bytes;
	msgs[1].flags = I2C_MSG_READ | I2C_MSG_STOP;

	return i2c_transfer(i2c_dev, &msgs[1],1, PCA9685_adrr | 0x01);
}
//pca9685设置频率
void PCA9685_setFreq(float freq)
{
	uint8_t prescale,oldmode,newmode;
	double prescaleval;
	int ret = 0;
	//freq *= 0.92;
	prescaleval = 25000000;
	prescaleval /= 4096;
	prescaleval /= freq;
	prescaleval -= 1;
	prescale = my_floor(prescaleval + 0.5f);
    ret = read_bytes(i2c_dev, PCA_Model, &oldmode, 1);
	newmode = (oldmode&0x7F) | 0x10;
    ret = write_bytes(i2c_dev,PCA_Model , &newmode, 1); 
    ret = write_bytes(i2c_dev,PCA9685_PRESCALE, &prescale, 1); 
    ret = write_bytes(i2c_dev,PCA_Model , &oldmode, 1); 
	k_busy_wait(5*1000);
    oldmode |= 0xa1;
    ret = write_bytes(i2c_dev,PCA_Model , &oldmode, 1); 
    if (ret) {
		LOG_INF("Error setFreq to pca9685 ! error code (%d)\n", ret);
		return;
	} 
}

int PCA9685_setPWM(uint8_t num,uint32_t on,uint32_t off)
{
    uint8_t data[4] = {(uint8_t)(on&0xFF) , (uint8_t)(on >> 8), (uint8_t)(off&0xFF), (uint8_t)(off >> 8)};
	int ret = write_bytes(i2c_dev,LED0_ON_L+4*num , &data[0], 1); 
	ret |= write_bytes(i2c_dev,LED0_ON_H+4*num , &data[1], 1); 
	ret |= write_bytes(i2c_dev,LED0_OFF_L+4*num , &data[2], 1); 
	ret |= write_bytes(i2c_dev,LED0_OFF_H+4*num , &data[3], 1); 
    if (ret) {
		LOG_INF("Error setPWM to pca9685 ! error code (%d)\n", ret);
		return -1;
	} else {
		return 0;
	}
}

void PCA9685_Init(float hz)
{
	
    uint8_t data[1] = {0x00};
    int ret = write_bytes(i2c_dev,PCA_Model, data, 1); 
    if (ret) {
		LOG_INF("Error writing to pca9685! error code (%d)\n", ret);
		return;
	} 
	PCA9685_setFreq(hz);

	k_busy_wait(100*1000);
	
}
void setAngle(uint8_t num,uint8_t angle)
{
	uint32_t off = 0;
	off = (uint32_t)(158 + angle*2.2);
	PCA9685_setPWM(num,0,off);

}

// void setAngle_360(uint8_t num,uint8_t angle_in)
// {
// 	uint32_t off = 0;
// 	uint8_t angle;
// 	fi(angle_in > 0){
// 		angle = angle_in*2;
// 	} 
// 	off = (uint32_t)(158 + angle*2.2);
// 	PCA9685_setPWM(num,0,off);

// }